using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.DirectX;

namespace Noein.Control
{
    public class CollisionController : Controller
    {
        // TODO using partition querying instead in the future
        private List<Object> objects = new List<Object>();

        private float cor = 1f;
        public float COR
        {
            get { return cor; }
            set { cor = value; }
        }

        public class CollisionRecord
        {
            public Object object1;
            public Object object2;
            public float scaledTime;

            //public int a;
            //public int b;
            //public CollisionRecord nextCollision;
        }
        private List<CollisionRecord> collisionRecords = new List<CollisionRecord>();
        public List<CollisionRecord> Collisions
        {
            get { return collisionRecords; }
        }

        public override void AttachTo(Object obj)
        {
            if (obj.Spatial == null)
                obj.Spatial = new Spatial();
            
            objects.Add(obj);
            obj.Controllers.Add(this);
        }

        public override void Update(Object obj, float dt)
        {
            // skip objects taht didn't move
            if (obj.Spatial.PreviousPosition == obj.Spatial.Position)
                return;

            #region Collision Detection

            bool checkAgainstStaticOnly = false;
            for (int i = 0; i < objects.Count; i++)
            {
                Object obj2 = objects[i];

                if (obj == obj2)
                {
                    checkAgainstStaticOnly = true;
                    continue;
                }

                if (checkAgainstStaticOnly && obj2.Spatial.PreviousPosition != obj2.Spatial.Position)
                    continue;

                float u0 = 0, u1 = 0;
                bool fail = false;
                bool collided = false;

                collided = Physics.Collision.Test.Sweep(obj, obj2, ref u0, ref u1, ref fail);
                if (fail)
                {
                    collided = Physics.Collision.Test.Intersect(obj, obj2, ref fail);
                }

                if (collided)
                {
                    // create collision record for the detected collision
                    CollisionRecord newCollsion = new CollisionRecord();
                    newCollsion.object1 = obj;
                    newCollsion.object2 = obj2;
                    newCollsion.scaledTime = u0;
                    //newCollsion.a = i;
                    //newCollsion.b = j;

                    collisionRecords.Add(newCollsion);

                    //if (collision == null)
                    //{
                    //    // on first collision detected
                    //    collision = newCollsion;
                    //}
                    //else if (collision.scaledTime > u0)
                    //{
                    //    // detected a collision that's earlier than all previous collisions
                    //    newCollsion.nextCollision = collision;
                    //    collision = newCollsion;
                    //}
                    //else
                    //{
                    //    // insert detected collision into the linked list ordered by normalized collision time
                    //    CollisionRecord traverse = collision;
                    //    while (traverse.nextCollision != null && traverse.nextCollision.scaledTime < u0)
                    //    {
                    //        traverse = traverse.nextCollision;
                    //    }
                    //    newCollsion.nextCollision = traverse.nextCollision;
                    //    traverse.nextCollision = newCollsion;
                    //}
                }
            }
            #endregion

            #region Collision Handling
            foreach (CollisionRecord collision in collisionRecords)
            {
                Vector3 loa;

                if (collision.object1.Physical != null)
                {
                    // HACK hardcode table collision response
                    if ((collision.object2.BoundingVolume as Physics.Collision.AABB) != null)
                    {
                        if (collision.object2.Spatial.Position.Y < 0f)
                        {
                            // bottom piece
                            loa = new Vector3(0, -1, 0);
                            float vrn = Vector3.Dot(collision.object1.Physical.Velocity, loa);

                            if (vrn > 0)
                            {
                                Vector3 newMomentum = collision.object1.Physical.Momentum;
                                newMomentum.Y *= -1f;
                                collision.object1.Physical.Momentum = newMomentum;
                            }
                        }
                        else if (collision.object2.Spatial.Position.X == 0)
                        {
                            loa = collision.object2.Spatial.Position.Z > 0 ?
                                    new Vector3(0, 0, 1) : new Vector3(0, 0, -1);
                            float vrn = Vector3.Dot(collision.object1.Physical.Velocity, loa);
                            if (vrn > 0)
                            {
                                // front and back piece
                                Vector3 newMomentum = collision.object1.Physical.Momentum;
                                newMomentum.Z *= -1f;
                                collision.object1.Physical.Momentum = newMomentum;
                            }
                        }
                        else
                        {
                            loa = collision.object2.Spatial.Position.X > 0 ?
                                new Vector3(1, 0, 0) : new Vector3(-1, 0, 0);
                            float vrn = Vector3.Dot(collision.object1.Physical.Velocity, loa);
                            if (vrn > 0)
                            {
                                // left and right piece
                                Vector3 newMomentum = collision.object1.Physical.Momentum;
                                newMomentum.X *= -1f;
                                collision.object1.Physical.Momentum = newMomentum;
                            }
                        }
                    }
                    else
                    {
                        loa = collision.object2.Spatial.Position - collision.object1.Spatial.Position;
                        loa.Normalize();

                        Vector3 relativeVelocity = collision.object1.Physical.Velocity -
                            collision.object2.Physical.Velocity;
                        float vrn = Vector3.Dot(relativeVelocity, loa);

                        if (vrn > 0)
                        {
                            Physics.Functions.ApplyImpulse(collision.object1, collision.object2, loa, cor);

                            // back stepping
                            collision.object1.Spatial.CorrectTo(collision.object1.Spatial.PreviousPosition +
                            (collision.object1.Spatial.Position - collision.object1.Spatial.PreviousPosition) *
                            collision.scaledTime);

                            collision.object2.Spatial.CorrectTo(collision.object2.Spatial.PreviousPosition +
                                (collision.object2.Spatial.Position - collision.object2.Spatial.PreviousPosition) *
                                collision.scaledTime);
                        }
                    }
                }
            }
            collisionRecords.Clear();

            #endregion
        }
    }
}
